﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using MSRoboticsFramework.ActuatorLayer.CompoundActuators;
using MSRoboticsFramework.ActuatorLayer.Actuators.Motors;

namespace WheeledTank.ActuatorLayer.Subsystems
{
    class VehicleSubsystem : ISteeringVehicle
    {
        private IGenericMotor MainMotor;
        private IGenericMotor SteeringMotor;

        private const float MAX_STEERING_ANGLE = Math.PI / 4;

        public VehicleSubsystem(IGenericMotor MainMotor, IGenericMotor SteeringMotor)
        {
            this.MainMotor = MainMotor;
            this.SteeringMotor = SteeringMotor;
        }

        #region ISteeringVehicle Members

        public float SteeringAngle
        {
            get
            {
                return SteeringMotor.Angle / MAX_STEERING_ANGLE;
            }
            set
            {
                SteeringMotor.Speed = 1;

                ///translate value into a limiting value if needed...
                if (value < -1) value = -1;
                if (value > 1) value = 1;

                SteeringMotor.Angle = MAX_STEERING_ANGLE * value;
            }
        }

        public float Speed
        {
            get
            {
                return MainMotor.Speed;
            }
            set
            {
                MainMotor.Speed = value;
            }
        }

        public void Forward()
        {
            MainMotor.Forward;
        }

        public void Backward()
        {
            MainMotor.Backward();
        }

        public void Brake()
        {
            MainMotor.Brake();
        }

        public void Coast()
        {
            MainMotor.Coast();
        }

        #endregion
    }
}
